#include "Img_deal.h"
#include "Prepared.h"

#define Boardsize 100
uint8_t Lrow[Boardsize], Lcol[Boardsize], Rrow[Boardsize], Rcol[Boardsize], Lsize, Rsize;

#define tmImage Imgdata

static inline uint8_t GetOSTU()
{
    uint16_t i, j;
    uint32_t Amount = 0;
    uint32_t PixelBack = 0;
    uint32_t PixelIntegralBack = 0;
    uint32_t PixelIntegral = 0;
    int32_t PixelIntegralFore = 0;
    int32_t PixelFore = 0;
    double OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差;
    int16_t MinValue, MaxValue;
    uint8_t Threshold = 0;
    uint8_t HistoGram[256] = {0}; //

    for (j = 0; j < 256; j++)
        HistoGram[j] = 0; // 初始化灰度直方图

    for (j = 0; j < IMG_ROW; j++)
    {
        for (i = 0; i < IMG_COL; i++)
        {
            HistoGram[tmImage[j][i]]++; // 统计灰度级中每个像素在整幅图像中的个数
        }
    }

    for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++)
        ; // 获取最小灰度的值
    for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--)
        ; // 获取最大灰度的值

    if (MaxValue == MinValue)
        return (uint8_t)MaxValue; // 图像中只有一个颜色
    if (MinValue + 1 == MaxValue)
        return (uint8_t)MinValue; // 图像中只有二个颜色

    for (j = MinValue; j <= MaxValue; j++)
        Amount += HistoGram[j]; //  像素总数

    PixelIntegral = 0;
    for (j = MinValue; j <= MaxValue; j++)
    {
        PixelIntegral += HistoGram[j] * j; // 灰度值总数
    }
    SigmaB = -1;
    for (j = MinValue; j < MaxValue; j++)
    {
        PixelBack = PixelBack + HistoGram[j];                                              // 前景像素点数
        PixelFore = Amount - PixelBack;                                                    // 背景像素点数
        OmegaBack = (double)PixelBack / Amount;                                            // 前景像素百分比
        OmegaFore = (double)PixelFore / Amount;                                            // 背景像素百分比
        PixelIntegralBack += HistoGram[j] * j;                                             // 前景灰度值
        PixelIntegralFore = PixelIntegral - PixelIntegralBack;                             // 背景灰度值
        MicroBack = (double)PixelIntegralBack / PixelBack;                                 // 前景灰度百分比
        MicroFore = (double)PixelIntegralFore / PixelFore;                                 // 背景灰度百分比
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore); // 计算类间方差
        if (Sigma > SigmaB)                                                                // 遍历最大的类间方差g //找出最大类间方差以及对应的阈值
        {
            SigmaB = Sigma;
            Threshold = (uint8_t)j;
        }
    }
    return Threshold; // 返回最佳阈值;
}

static inline void Get_OSTU_Img(uint8_t Thresh)
{
    for (uint8_t i = 0; i < IMG_ROW; i++)
        for (uint8_t j = 0; j < IMG_COL; j++)
            if (ImageUsed[i][j] > Thresh)
                PerImg[i][j] = 255;
            else
                PerImg[i][j] = 0;
}

/*完成摄像头初始化后，调用一次ImagePerspective_Init，此后，直接调用ImageUsed   即为透视结果*/
static inline void show_PerImg()
{
    for (uint8_t i = 0; i < IMG_ROW; i++)
        for (uint8_t j = 0; j < IMG_COL; j++)
        {
            if (PerImg_ip[i][j] == &BlackColor)
                continue;
            else
            {
                PerImg[i][j] = ImageUsed[i][j];
            }
        }
}
static inline uint8_t Find_Start_Point(unsigned char *x1, unsigned char *y1, unsigned char *x2, unsigned char *y2, int block_size, int thres)
{
    unsigned char l_flag = 1, r_flag = 1;
    int half = block_size / 2, x, y;
    for (y = IMG_ROW - 2 - half; l_flag && y > IMG_ROW - 20 - half; y--)
    {
        for (x = IMG_COL / 2; x > 2; x--)
        {
            if (PerImg_ip[y][x - 3] == &BlackColor)
                break;
            if (ImageUsed[y][x + 2] + ImageUsed[y][x + 1] - ImageUsed[y][x - 2] - ImageUsed[y][x - 1] > thres)
            {
                *x1 = x, *y1 = y;
                l_flag = 0;
                break;
            }
        }
    }
    for (y = IMG_ROW - 2 - half; r_flag && y > IMG_ROW - 20 - half; y--)
    {
        for (x = IMG_COL / 2; x < IMG_COL - 2; x++)
        {
            if (PerImg_ip[y][x + 3] == &BlackColor)
                break;
            if (ImageUsed[y][x + 2] + ImageUsed[y][x + 1] - ImageUsed[y][x - 2] - ImageUsed[y][x - 1] < -thres)
            {
                *x2 = x, *y2 = y;
                r_flag = 0;
                break;
            }
        }
    }
    if (!l_flag && !r_flag)
        return 3; // 0011
    else if (!r_flag)
        return 2; // 0010
    else if (!l_flag)
        return 1; // 0001
    else
        return 0;
}
const signed char rowfront[4] = {-1, 0, 1, 0};
const signed char colfront[4] = {0, 1, 0, -1};
const signed char rowleftfront[4] = {-1, -1, 1, 1};
const signed char colleftfront[4] = {-1, 1, 1, -1};
const signed char rowrightfront[4] = {-1, 1, 1, -1};
const signed char colrightfront[4] = {1, 1, -1, -1};
static inline void Find_R(unsigned char rows[], unsigned char cols[], int block_size, unsigned char *size, unsigned char x, unsigned char y, int clip_value)
{
    unsigned char step = 0, dir = 0, turn = 0;
    int half = block_size / 2;
    while (step < *size && turn < 4 && half + 1 < x && x < IMG_COL - 2 - half && 6 < y && y < IMG_ROW - 1 - half && PerImg_ip[y][x] != &BlackColor && PerImg_ip[y][x - 2] != &BlackColor && PerImg_ip[y][x + 2] != &BlackColor)
    {
        int local_thres = 0;
        for (int dy = -half; dy <= half; dy++)
        {
            for (int dx = -half; dx <= half; dx++)
            {
                local_thres += ImageUsed[y + dy][x + dx];
            }
        }
        local_thres /= block_size * block_size;
        local_thres -= clip_value;
        unsigned char front_value = ImageUsed[y + rowfront[dir]][x + colfront[dir]];
        unsigned char frontright_value = ImageUsed[y + rowrightfront[dir]][x + colrightfront[dir]];
        if (front_value < local_thres)
        {
            dir = (dir + 3) % 4;
            turn++;
        }
        else if (frontright_value < local_thres)
        {
            x += colfront[dir];
            y += rowfront[dir];
            rows[step] = y;
            cols[step] = x;
            step++;
            turn = 0;
        }
        else
        {
            y += rowrightfront[dir];
            x += colrightfront[dir];
            dir = (dir + 1) % 4;
            rows[step] = y;
            cols[step] = x;
            step++;
            turn = 0;
        }
    }
    *size = step;
}
static inline void Find_L(unsigned char rows[], unsigned char cols[], int block_size, unsigned char *size, unsigned char x, unsigned char y, int clip_value)
{
    unsigned char step = 0, dir = 0, turn = 0;
    int half = block_size / 2;
    while (step < *size && turn < 4 && half + 1 < x && x < IMG_COL - 2 - half && 6 < y && y < IMG_ROW - 1 - half && PerImg_ip[y][x] != &BlackColor && PerImg_ip[y][x - 2] != &BlackColor && PerImg_ip[y][x + 2] != &BlackColor)
    {
        int local_thres = 0;
        for (int dy = -half; dy <= half; dy++)
        {
            for (int dx = -half; dx <= half; dx++)
            {
                local_thres += ImageUsed[y + dy][x + dx];
            }
        }
        local_thres /= block_size * block_size;
        local_thres -= clip_value;

        unsigned char front_value = ImageUsed[y + rowfront[dir]][x + colfront[dir]];
        unsigned char frontleft_value = ImageUsed[y + rowleftfront[dir]][x + colleftfront[dir]];
        if (front_value < local_thres)
        {
            dir = (dir + 1) % 4;
            turn++;
        }
        else if (frontleft_value < local_thres)
        {
            x += colfront[dir];
            y += rowfront[dir];
            rows[step] = y;
            cols[step] = x;
            step++;
            turn = 0;
        }
        else
        {
            y += rowleftfront[dir];
            x += colleftfront[dir];
            dir = (dir + 3) % 4;
            rows[step] = y;
            cols[step] = x;
            step++;
            turn = 0;
        }
    }
    *size = step;
}
static inline void Draw_Board(uint8_t row[], uint8_t col[], uint8_t size)
{
    for (uint8_t i = 0; i < size; i++)
    {
        Draw_Color(col[i], row[i], BLUE);
    }
}
void ImageDeal_main()
{
     memset(PerImg, 0, sizeof(PerImg));
     cout << (int)GetOSTU() << endl;
     Get_OSTU_Img(GetOSTU());
   /* uint8_t lx, ly, rx, ry, return_state;
    return_state = Find_Start_Point(&lx, &ly, &rx, &ry, 3, 100);
    if (return_state & 0x01)
    {
        Lsize = Boardsize;
        Find_L(Lrow, Lcol, 3, &Lsize, lx, ly, 10);
    }
    if (return_state & 0x02)
    {
        Rsize = Boardsize;
        Find_R(Rrow, Rcol, 3, &Rsize, rx, ry, 10);
    }
    show_PerImg();
    if (return_state & 0x01)
    {
        Draw_Color(lx,ly,RED);
        Draw_Board(Lrow, Lcol, Lsize);
        cout << "Lsize: " << (int)Lsize << endl;
    }
    if (return_state & 0x02)
    {
         Draw_Color(rx,ry,RED);
        Draw_Board(Rrow, Rcol, Rsize);
        cout << "Rsize: " << (int)Rsize << endl;
    }*/
}
void reset_staus()
{
}